#include "app.h"

#include <cstdio>
#include <GL\AntTweakBar.h>

#include <utils.hpp>
using namespace siut::simd;
#include <Vec2f.hpp>
using siut::simd::Vec2f;

#include "input.h"
#include "shapes3d.h"
#include "fonts\bitmap_font.h"
#include "random.h"

#include "collision_primitive.h"
#include "collisiondetector.h"
#include "physics_object.h"

App::App() : GLFWApp(), doSelectionOnRender(false), cameraMoveable(false)
{
	lightPosition[0] = 25.0f;
	lightPosition[1] = 25.0f;
	lightPosition[2] = 25.0f;
	lightPosition[3] = 1.0f;

	quat[0] = 0.0f;
	quat[1] = 0.0f;
	quat[2] = 0.0f;
	quat[3] = 1.0f;

	BitmapFont::Init();
	simIterations = 0;

	/************************************************************************/
	/*           Create and setup collision objects, then reset state       */
	/************************************************************************/
	cData.contactArray = contacts; // TODO make easier than pointer assignment...

	for( int i=0; i<MAX_BALLS + MAX_BOXES; i++)
	{
		TorPhysix::PhysicsObject *phyObj;
		if( i < MAX_BALLS)
		{
			Ball *b = new Ball;
			phyObj = b;
			ballList.push_back( b );
		}else
		{
			Box *b = new Box;
			phyObj = b;
			boxList.push_back( b );
		}
		sceneObjects.push_back( phyObj );
		physObjList.push_back( phyObj );
	}

	myManipulator.addHandlesToList(sceneObjects);
	
	myPerfTimers.samples = 0;

	simRunning = false;
	resetPhysics();
	resetCamera();
	initGUI();
	glfwSetWindowTitle("TorPhy editor");
	glfwSwapInterval(0); // vsync
	mainloop();
}



void App::update()
{
	if ( myPerfTimers.samples >= 100 )
	{	
		myPerfTimers.samples = 0;
		myPerfTimers.render = myPerfTimers.renderAccum / 100.0f;
		myPerfTimers.renderAccum = 0;

		myPerfTimers.update = myPerfTimers.updateAccum / 100.0f;
		myPerfTimers.updateAccum = 0;

		myPerfTimers.genContacts = myPerfTimers.genContactsAccum / 100.0f;
		myPerfTimers.genContactsAccum = 0;
	}
	myPerfTimers.samples++;

	float camDx = ( Input::keyDown('D') -  Input::keyDown('A') ) * 10.0f * (float)dt;
	float camDz = ( Input::keyDown('S') -  Input::keyDown('W') ) * 10.0f * (float)dt;
	myCamera.update(camDx, 0.0f, camDz, 0.0f, 0.0f);

	// Apply forces
	// TODO. bad practice..
	if ( myManipulator.selectedSceneObj )	
	{
		// TODO. don't like static cast if i can help it...
		TorPhysix::PhysicsObject *phyObj = static_cast<TorPhysix::PhysicsObject*>(myManipulator.selectedSceneObj);
		//selectedSceneObject.physicsObject.applyRotation( x,y,z );
		float rotationZ = ( Input::keyDown(GLFW_KEY_RIGHT) -  Input::keyDown(GLFW_KEY_LEFT) ) * 18.0f;
		float rotationX = ( Input::keyDown(GLFW_KEY_UP) -  Input::keyDown(GLFW_KEY_DOWN) ) * 18.0f;
		phyObj->collisionPrimitive->body->setRotation( Vec3f(rotationX, 0.0f, rotationZ) );
		if ( Input::keyDown(GLFW_KEY_RCTRL) ) phyObj->collisionPrimitive->body->setRotation( Vec3f( 0.0, 0.0,  0.0) );
	}

	if ( Input::keyHit(GLFW_KEY_SPACE) ) stepOneFrame = true;


	if ( stepOneFrame || simRunning ) 
	{
		real timestep = (real) 0.01;
		if ( stepOneFrame )
		{
			timestep =  (real) 0.01;
			stepOneFrame = false;
		}
		else if ( simRunning ) timestep = (real)dt;

		if ( timestep <= 0.0f ) return;
		if ( timestep > 0.05f )  timestep = 0.05f;

		double updateStart = glfwGetTime();
		updateObjects(timestep);
		myPerfTimers.updateAccum += glfwGetTime() - updateStart;

		// Generate contacts for resting objects and new collisions
		double genStart = glfwGetTime();
		generateContacts();
		
		// Resolve detected contacts
		resolver.resolveContacts( cData.contactArray, cData.contactCount, timestep );
		myPerfTimers.genContactsAccum += glfwGetTime() - genStart;

		simIterations++;

	}
}

void App::render()
{
	double renderStart = glfwGetTime();
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	setDepthProjection();
	glLoadIdentity();

	glEnable(GL_DEPTH_TEST);

	//if ( myManipulator.selectedSceneObj != 0x0 && simRunning )
	//{
	//	const Vec3f &pos = myManipulator.selectedSceneObj->vGetPosition();
	//	//const Vec3f &velDir = normalize(  myManipulator.selectedSceneObj->collisionObject.body->getVelocity() );
	//	const Vec3f lookat = pos;
	//	const Vec3f camPosition = pos - Vec3f(0.0f, -20.0f, +10.0f);
	//	const Vec3f camTarget = pos;

	//	gluLookAt( camPosition.x(), camPosition.y() , camPosition.z(), 
	//		camTarget.x(), camTarget.y() , camTarget.z(), 
	//		0.0, 1.0, 0.0 );
	//}
	//else
	{
		glMultMatrixf( myCamera.getMatrix() );
	}

	// TODO render to seperate buffer. Learn how to use MRT - multiple render targets? 
	if ( doSelectionOnRender )
	{
		doSelectionOnRender = false;
		selectObjectUnderCursor();
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	}
	myManipulator.render();

	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
	
	glLightfv( GL_LIGHT0, GL_POSITION, lightPosition );
	glEnable(GL_COLOR_MATERIAL);

	// TODO improve performance... minimize state changes
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	glDisable(GL_LIGHTING);
	glDisable(GL_DEPTH_TEST);
	//glDisable(GL_COLOR_MATERIAL);
	glColor4f(0,0,0,0.1f);

	std::vector<TorPhysix::PhysicsObject*>::iterator it;
	for(it = physObjList.begin(); it != physObjList.end(); ++it)
	{
		(*it)->vRenderShadow();
	}



	glDisable(GL_BLEND);
	glEnable(GL_DEPTH_TEST);


	glEnable(GL_BLEND);
	glColor4ub(32,64,192, 128);
	glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	for(it = physObjList.begin(); it != physObjList.end(); ++it)
	{
		if(  (*it) == myManipulator.selectedSceneObj )
			(*it)->vRender();
	}
	glDisable(GL_BLEND);

	glEnable(GL_LIGHTING);
	
	glColor3ub(32,64,192);
	for(it = physObjList.begin(); it != physObjList.end(); ++it)
	{
		if(  (*it) != myManipulator.selectedSceneObj )
			(*it)->vRender();
	}

	for(it = physObjList.begin(); it != physObjList.end(); ++it)
	{
		if( (*it) == myManipulator.selectedSceneObj )
		{
			glColor3f(1.0, 0.5f + 1.0f * Random::nrnd(), 0.0);
			glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
			(*it)->vRender();
			glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
		}else
		{
			glColor3ub(192,64,192);
			(*it)->vRender();
		}
	}

	// Draw with no shading
	glDisable(GL_LIGHTING);
	
	renderDebugInfo();

	//setOrthoProjection(); // Ant does this itself
	TwDraw();  // draw the tweak bar(s)	

	myPerfTimers.renderAccum += glfwGetTime() - renderStart;
}

void App::renderDebugInfo()
{
	// Recalculate the contacts, so they are current (in case we're
	// paused, for example).
	generateContacts();

	// Render the contacts, if required
	for (unsigned i = 0; i < cData.contactCount; i++)
	{
		// Inter body contacts are in green, floor contacts are red.
		if (contacts[i].body[1]) {
			glColor3f(0,1,0);
		} else {
			glColor3f(1,0,0);
		}
			
		glPushMatrix();
		Vec3f correct = contacts[i].contactPoint;// - contacts[i].contactNormal;
		glTranslatef( correct.x(), correct.y(), correct.z() );

		Shapes3D::DrawSphere(0.1f, 12,12);
		glPopMatrix();
		
		Vec3f vec = contacts[i].contactPoint;
		glBegin(GL_LINES);
		glVertex3f(vec.x(), vec.y(), vec.z());

		vec += contacts[i].contactNormal * 0.1f;
		glVertex3f(vec.x(), vec.y(), vec.z());
		glEnd();
	}

	std::vector<TorPhysix::PhysicsObject*>::iterator it;
	for(it = physObjList.begin(); it != physObjList.end(); ++it)
	{
		(*it)->vRenderDebug();
	}

}

void App::generateContacts()
{
	// Ground
	TorPhysix::CollisionPlane plane;
	plane.direction = Vec3f(0.0f,1.0f,0.0f);
	plane.offset = 0;

	// Setup contacts
	cData.reset(maxContacts);
	cData.friction = (real)0.7f;
	//cData.friction = (real)0.6f;
	cData.restitution = (real)0.6;
	//cData.restitution = (real)0.6;
	//cData.tolerance = (real)0.01;
	cData.tolerance = (real)0.0001;

	for( int i=0; i<MAX_BALLS; ++i)
	{
		for( int j=0; j<MAX_BOXES; ++j)
		{
			if (!cData.hasMoreContacts()) return;
			TorPhysix::CollisionDetector::boxAndSphere( *(boxList[j]->collisionBox), *(ballList[i]->collisionSphere), &cData ); 
		}

		for( int j=i+1; j<MAX_BALLS; ++j)
		{
			if (!cData.hasMoreContacts()) return;
			TorPhysix::CollisionDetector::sphereAndSphere( *(ballList[i]->collisionSphere), *(ballList[j]->collisionSphere), &cData );
		}
	}


}

void App::updateObjects( real duration )
{
	std::vector<TorPhysix::PhysicsObject*>::iterator it;
	for(it = physObjList.begin(); it != physObjList.end(); ++it)
	{
		(*it)->update(duration);
	}
}

void App::resetPhysics()
{
	float x = 0;
	float y = 0;
	float z = 0;
	float dist = 1.2f;
	// Arrange in a cube, get cube root of number of balls to decide num to make on a side
	//int limit = (int) real_pow( int(MAX_BALLS-1), 1.0f/3.0f )-1;
	int limit = 2;

	Vec3f position = Vec3f(0.0f,0.0f,0.0f);
	Vec3f halfSize = Vec3f(0.5f, 0.5f, 0.5f);
	real mass = (real) 5.0;
	Quatf orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
	Vec3f acceleration = Vec3f(0.0f, -15.0f, 0.0f );

	for(int i=0; i<MAX_BALLS+MAX_BOXES; i++)
	{
		if( i < MAX_BALLS)
		{
			physObjList[i]->vSetState( -limit*0.5f*dist + x * dist, 5.0f + y*dist, -limit*0.5f*dist +  z * dist, orientation, halfSize, 1.0f, acceleration);
			physObjList[i]->collisionPrimitive->body->setPosition(physObjList[i]->collisionPrimitive->body->getPosition() + Vec3f(5.0f) );
			physObjList[i]->collisionPrimitive->body->calculateDerivedData();
			physObjList[i]->collisionPrimitive->calculateInternals();

			x++;
			if( x > limit)
			{
				x = 0;
				z++;
			}
			if( z > limit)
			{
				z = 0;
				y++;
			}
		}
		else
		{
			position = Vec3f(0.0f, 1.5f, 0.0f);
			halfSize = Vec3f(0.5f, 0.5f, 0.5f);
			mass = (real) 5.0;
			orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
			acceleration = Vec3f(0.0f, 0.0f, 0.0f );

			if( i == MAX_BOXES-1)
			{
				position.is(5,5,5);
				halfSize = Vec3f(4.0, 0.5, 4.0);
				// High mass to make it near immovable my other impacts
				mass = 1e10f;
				acceleration = Vec3f(0.0f, 0.0f, 0.0f );
				orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
			}
			else if( i == MAX_BOXES-2)
			{
				position.is(5,2,-6);
				halfSize = Vec3f(2.5, 0.5, 6.0);

				// High mass to make it near immovable my other impacts
				mass = 1e6f;
				acceleration = Vec3f(0.0f, 0.0f, 0.0f );
				orientation = Quatf(0.25f, 1.0f, 0.0f, 0.0f) ;
			}else
			{
				float radius = 10;
				float ang = 1 * 6.28f * i / float(MAX_BOXES);
				float x = cos(ang) * radius;
				float y = 0.8f;
				float z = sin(ang) * radius;
				position.is(x,y,z);
				halfSize = Vec3f(0.5, 0.5, 0.5);
				//acceleration = Vec3f(0.0f, -9.81f, 0);
				acceleration = Vec3f(0.0f, 0.0f, 0.0f );
				//orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
				FromAxis(orientation, Vec3f(0.0f,1.0f,0.0f), -ang);
				mass = 1e8f;
			}
			physObjList[i]->vSetState( position.x(), position.y(), position.z(), orientation, halfSize, mass, acceleration);
		}
	}

	// Reset the contacts
	cData.contactCount = 0;

	// Reset simulation step count
	simIterations = 0;
}

void App::selectObjectUnderCursor()
{
	// turn off texturing, lighting and fog
	glDisable(GL_TEXTURE_2D);
	glDisable(GL_FOG);
	glDisable(GL_LIGHTING);
	glDisable(GL_COLOR_MATERIAL);

	// render every object in our scene
	std::list<SceneObject *>::iterator itr;
	for(itr = sceneObjects.begin(); itr != sceneObjects.end(); ++itr)
	{
		if( myManipulator.selectedSceneObj != (*itr) ) 
			(*itr)->vPicking();
	}

	// get color information from frame buffer
	unsigned char pixel[3];
	GLint viewport[4];
	glGetIntegerv(GL_VIEWPORT, viewport);
	glReadPixels(selectionTarget.x(), viewport[3] - selectionTarget.y(), 1, 1, GL_RGB, GL_UNSIGNED_BYTE, pixel);

	// now our picked screen pixel color is stored in pixel[3]
	// so we search through our object list looking for the object that was selected
	bool foundSceneObj = false;
	bool foundManipulator = false;
	for(itr = sceneObjects.begin(); itr != sceneObjects.end(); ++itr)
	{
		if( myManipulator.selectedSceneObj == (*itr) ) continue;

		if((*itr)->m_colorID[0] == pixel[0] && (*itr)->m_colorID[1] == pixel[1] && (*itr)->m_colorID[2] == pixel[2])
		{
			// flag object as selected
			setSelected( (*itr) );

			if ( (*itr)->vIsManipulator() )
			{
				foundManipulator = true;
			}else
			{
				foundSceneObj = true;
			}

			break;
		}
	}

	if( !foundManipulator )
	{
		myManipulator.selectedTool = NULL;
	}

}

void App::setSelected( SceneObject *sceneObject )
{
	if( sceneObject->vIsManipulator()  )
	{
		myManipulator.activateTool(sceneObject);
		//sceneObject->vOnClick(); // TODO better design afterall?
	}else
	{
		myManipulator.setSceneObject(sceneObject);
	}
}

App::~App()
{
	TwTerminate();
}

void TW_CALL _resetPhysicsCB(void * clientData)
{ 
	static_cast<App *>(clientData)->resetPhysics(); // pointer is stored in clientData
}

void TW_CALL _resetCameraCB(void * clientData)
{ 
	static_cast<App *>(clientData)->resetCamera(); // pointer is stored in clientData
}

void App::initGUI()
{
	// Initialize AntTweakBar
	TwInit(TW_OPENGL, NULL);
	TwWindowSize(xres, yres);

	TwBar *myBar;
	myBar = TwNewBar("Torb-Editor options:");
	TwAddVarRW(myBar, "LightDir", TW_TYPE_DIR3F, &lightPosition, "group=Engine");
	TwAddVarRO(myBar, "Rotation", TW_TYPE_QUAT4F, &quat, "group=Engine");
	TwAddVarRO(myBar, "Space - singlestep. 1 - run/stop", TW_TYPE_INT32, &simIterations, " group=Engine label='sim iterations' ");
	TwAddVarRO(myBar, "fps", TW_TYPE_DOUBLE, &fps, " group=Engine label='fps' ");
	TwAddVarRO(myBar, "position", TW_TYPE_INT32, &resolver.positionIterationsUsed, " group=Resolver label='pos iter' ");
	TwAddVarRO(myBar, "velocity", TW_TYPE_INT32, &resolver.velocityIterationsUsed, " group=Resolver label='vel iter' ");
	TwAddVarRO(myBar, "#contacts", TW_TYPE_INT32, &cData.contactCount, " group=Generator label='num contacts' ");
	TwAddVarRO(myBar, "#max", TW_TYPE_INT32, &maxContacts, " group=Generator label='max contacts' ");
	// TODO warn if vel or pos iters > max -> misses
	TwAddButton(myBar, "reset cam", _resetCameraCB, this, " group=Resetting label='reset cam' ");
	TwAddButton(myBar, "reset physics", _resetPhysicsCB, this, " group=Resetting label='reset physics' ");
	TwAddVarRW(myBar, "sim running", TW_TYPE_BOOLCPP, &simRunning, "group=Resetting");

	TwAddVarRO(myBar, "gen contacts", TW_TYPE_DOUBLE, &(myPerfTimers.genContacts), " group=Performance ");
	TwAddVarRO(myBar, "update dyn", TW_TYPE_DOUBLE, &(myPerfTimers.update), " group=Performance ");
	TwAddVarRO(myBar, "render", TW_TYPE_DOUBLE, &(myPerfTimers.render), " group=Performance ");

	TwDefine(" 'Torb-Editor options:' refresh=0.05 "); // refresh the bar every . seconds
}

void App::resetCamera()
{
	myCamera.posx = 0;
	myCamera.posy = 10;
	myCamera.posz = -15;
	myCamera.hang = 3.14f;
	myCamera.vang = 0.0f;
}

void App::applyManipulatorToSelected()
{
	std::list<SceneObject*>::iterator it;
	for(it = sceneObjects.begin(); it != sceneObjects.end(); ++it )
	{
		if ( (*it)->vIsManipulator() )
		{
			myManipulator.useActiveTool(); // prev design allowed multiple tools at once...
			//(*it)->vInteractWith( myManipulator.selectedSceneObj );
		}	
	}
}

void App::onMouseButton( const int x, const int y, const int button, const bool pressed )
{
	TwMouseButton( pressed? TW_MOUSE_PRESSED:TW_MOUSE_RELEASED, (button==GLFW_MOUSE_BUTTON_LEFT) ? TW_MOUSE_LEFT:TW_MOUSE_RIGHT);

	if( button == GLFW_MOUSE_BUTTON_RIGHT && pressed)
	{
		cameraMoveable = true;
	}
	else if( button == GLFW_MOUSE_BUTTON_RIGHT && !pressed)
	{
		cameraMoveable = false;
	}

	if( button == GLFW_MOUSE_BUTTON_LEFT  )
	{
		if( pressed )
		{
			doSelectionOnRender = true;
			selectionTarget = Vec2i(x,y);

			myManipulator.isSelected = true;
		}else
		{
			myManipulator.isSelected = false;
		}	
	}

	myManipulator.update(0.0);
}

void App::onMouseMove( const int x, const int y, const int deltaX, const int deltaY )
{
	TwMouseMotion( x, y );

	if ( cameraMoveable ) 
	{
		const float mouseSense = 0.005f;
		myCamera.hang -= deltaX * mouseSense;
		myCamera.vang += deltaY * mouseSense;
	}

	float moveSpeed = deltaY * 0.05f;
	if( Input::keyDown(GLFW_KEY_RSHIFT) || Input::keyDown(GLFW_KEY_LSHIFT) ) moveSpeed *= 2;
	myManipulator.update(moveSpeed);

	if( myManipulator.isSelected )
	{
		applyManipulatorToSelected();
	}
}

void App::onSize( const int w, const int h )
{
	TwWindowSize(w, h);
	GLFWApp::onSize(w,h);
}

void App::onKey( const int key, const bool pressed )
{
	if( pressed )
		TwKeyPressed(key==GLFW_KEY_BACKSPACE?TW_KEY_BACKSPACE:key, key==GLFW_KEY_RSHIFT?TW_KMOD_SHIFT:TW_KMOD_NONE );

	if ( key == 'Q') 
	{
		myManipulator.setScaling(pressed);
	}


	GLFWApp::onKey(key, pressed);
}

void App::onClose()
{
	GLFWApp::onClose(); // still want it to close app when we're done
}
